/**
* @file LpsData.cpp in SimulateLPS
* @author rebeater
* @comment
* Create on 3/23/22 10:50 AM
* @version 1.0
**/
#include <memory.h>
#include "LpsData.h"

#define CRC32_POLY 0x04C11DB7U
#define CRC32_START 0xFFFFFFFFU
uint32_t crc_checksum(const uint8_t *indata, int num_bytes) {
  /*软件校验*/
  uint32_t crc = CRC32_START;
  const uint8_t *p = indata;
  for (int i = 0; i < num_bytes; i++) {
    crc = crc ^ (*p++ << 24U);
    for (int bit = 0; bit < 8; bit++) {
      if (crc & (1LU << 31U)) crc = (crc << 1U) ^ CRC32_POLY;
      else crc = (crc << 1U);
    }
  }
  return crc;
}
LpsRaw FromGnssData(GnssData &gnss_data){
  LpsRaw raw{};
  raw.week = gnss_data.week;
  raw.gpst = gnss_data.gpst;
  raw.height = gnss_data.height;
  raw.lat = gnss_data.lat;
  raw.lon	= gnss_data.lon;
  raw.ns = gnss_data.ns;
  raw.mode = 4;
  raw.pos_std[0] = 1024 * gnss_data.pos_std[0];
  raw.pos_std[1] = 1024 * gnss_data.pos_std[1];
  raw.pos_std[2] = 1024 * gnss_data.pos_std[2];
  raw.header = 0XAA55AA55;
  raw.tail = 0x0C3C0C3C;
  uint32_t crc = crc_checksum((uint8_t *)&raw,sizeof(raw)-4);
  uint8_t *p = reinterpret_cast<uint8_t *>(&raw.check);
  for(int i = 0; i < 4; i++){
    *p = (crc>>(8*(3-i)))&0xff;
    p++;
  }
  return raw;
}

